#include <chrono>
#include <memory>
#include <string>
#include <cmath>  // 用于检测NaN

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"
#include "std_msgs/msg/bool.hpp"

using namespace std::chrono_literals;

class GpsSignalMonitor : public rclcpp::Node
{
public:
    GpsSignalMonitor()
        : Node("gps_signal_monitor_node")
    {
        // 声明参数，支持灵活配置
        this->declare_parameter<std::string>("gps_topic", "/fix");
        this->declare_parameter<std::string>("signal_topic", "/gps_signal_state");
        this->declare_parameter<std::string>("align_all_topic", "/align_all");
        this->declare_parameter<double>("gps_timeout_sec", 2.0);

        // 获取参数
        this->get_parameter("gps_topic", gps_topic_);
        this->get_parameter("signal_topic", signal_topic_);
        this->get_parameter("align_all_topic", align_all_topic_);
        this->get_parameter("gps_timeout_sec", gps_timeout_sec_);

        last_msg_time_ = this->now();

        // 订阅GPS话题
        gps_sub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            gps_topic_, 10,
            std::bind(&GpsSignalMonitor::gps_callback, this, std::placeholders::_1));

        // 创建GPS信号状态发布器
        signal_pub_ = this->create_publisher<std_msgs::msg::Bool>(signal_topic_, 1);
        control_pub_ = this->create_publisher<std_msgs::msg::Bool>(align_all_topic_, 1);

        // 创建两个定时器：
        // 1. 内部定时器（如100ms周期）计算当前GPS状态
        // 2. 发布定时器每秒发布一次当前GPS状态
        internal_timer_ = this->create_wall_timer(
            100ms, std::bind(&GpsSignalMonitor::internal_timer_callback, this));
        publish_timer_ = this->create_wall_timer(
            1s, std::bind(&GpsSignalMonitor::publish_timer_callback, this));

        // 初始化为无信号
        update_gps_state(false);

        RCLCPP_INFO(this->get_logger(),
                    "GPS信号监控节点已启动，订阅话题: %s，发布话题: %s，标定话题: %s，超时时间: %.2f秒，状态发布周期: 1秒",
                    gps_topic_.c_str(), signal_topic_.c_str(), align_all_topic_.c_str(),
                    gps_timeout_sec_);
    }

private:
    // GPS消息订阅回调函数
    void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg)
    {
        // 检查数据是否为无效（如nan或status异常），如果是则立即判定无信号
        if (
            std::isnan(msg->latitude) ||
            std::isnan(msg->longitude) ||
            std::isnan(msg->altitude) ||
            msg->status.status < 0 // status -1 通常为无信号
        ) {
            update_gps_state(false);
            last_msg_time_ = this->now(); // 仍然更新时间，避免定时器误判为超时
            RCLCPP_WARN(this->get_logger(), "检测到无效GPS数据，已判定为无信号");
            return;
        }
        // 否则，正常更新时间
        last_msg_time_ = this->now();
        update_gps_state(true);
    }

    // 内部定时器回调：更新当前GPS状态
    void internal_timer_callback()
    {
        auto now = this->now();
        // 仅在没有收到"无效"消息的情况下才因超时判定无信号
        if ((now - last_msg_time_).seconds() >= gps_timeout_sec_) {
            update_gps_state(false);
        }
    }

    // 发布定时器回调：每秒发布一次当前GPS状态
    void publish_timer_callback()
    {
        publish_signal(current_gps_state_);
    }

    // 仅保存最新状态，不发布
    void update_gps_state(bool state)
    {
        current_gps_state_ = state;
    }

    // 始终发布，但仅在状态变化时打印日志和发布控制消息
    void publish_signal(bool state)
    {
        auto msg = std_msgs::msg::Bool();
        msg.data = state;
        signal_pub_->publish(msg);
        RCLCPP_INFO(this->get_logger(), "发布GPS信号状态: %s", state ? "有信号" : "无信号");
        if (state != last_state_)
        {
            // 若state为true，发布在线标定控制消息；若state为false，发布历史标定控制消息
            auto control_msg = std_msgs::msg::Bool();
            if (state)
            {
                control_msg.data = false; // 在线标定
            }
            else
            {
                control_msg.data = true; // 历史标定
            }

            control_pub_->publish(control_msg);
            RCLCPP_INFO(this->get_logger(), "发布标定控制状态: %s", state ? "在线标定" : "历史标定");
            // 更新最后状态
            last_state_ = state;
        }
    }

    // 参数与成员变量
    std::string gps_topic_;
    std::string signal_topic_;
    std::string align_all_topic_;
    double gps_timeout_sec_;

    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_sub_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr signal_pub_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr control_pub_;

    rclcpp::TimerBase::SharedPtr internal_timer_;
    rclcpp::TimerBase::SharedPtr publish_timer_;

    rclcpp::Time last_msg_time_;
    bool current_gps_state_ = false;
    bool last_state_ = false;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<GpsSignalMonitor>());
    rclcpp::shutdown();
    return 0;
}